#include "bsp_inc.h"

typedef struct MotorDemo_TUnionTag MotorDemo_TUnion;
struct MotorDemo_TUnionTag
{
    Bsp_THandle handle[Bsp_kMotorIdMax];
};

MotorDemo_TUnion g_motor_demo_union;

extern void Delay(UInt32 ms);

void MotorDemo_Init(void)
{
    MotorDemo_TUnion* self = &g_motor_demo_union;

    for (Int32 i = 0; i < Bsp_kMotorIdMax; i++)
    {
        self->handle[i] = Bsp_MotorCreate(i);
        Bsp_MotorOpen(self->handle[i]);
        Bsp_MotorSetDirection(self->handle[i], False);
        Bsp_MotorSetSpeed(self->handle[i], 10);
        Bsp_MotorStart(self->handle[i]);
    }

    Delay(5000);
    
    for (Int32 i = 0; i < Bsp_kMotorIdMax; i++)
    {
//        self->handle[i] = Bsp_MotorCreate(i);
//        Bsp_MotorOpen(self->handle[i]);
        Bsp_MotorSetDirection(self->handle[i], False);
        Bsp_MotorSetSpeed(self->handle[i], 100);
//        Bsp_MotorStart(self->handle[i]);
    }

    Delay(5000);

    for (Int32 i = 0; i < Bsp_kMotorIdMax; i++)
    {
        Bsp_MotorStop(self->handle[i]);
        Bsp_MotorClose(self->handle[i]);
        Bsp_MotorFree(self->handle[i]);
    }
}
